package org.nashua.tt151;

import com.sun.squawk.util.MathUtils;
import edu.wpi.first.wpilibj.DigitalOutput;
import edu.wpi.first.wpilibj.Jaguar;
import edu.wpi.first.wpilibj.Servo;
import edu.wpi.first.wpilibj.SimpleRobot;
import edu.wpi.first.wpilibj.Timer;
import org.nashua.tt151.Controller.*;
import org.nashua.tt151.RobotMap.Hoop;

public class Main extends SimpleRobot {
    /* *************************** INITIALIZATION *************************** */
    // DualAction Controllers
    public static final DualAction driver = new DualAction(1);
    public static final DualAction shooter = new DualAction(2);
    // Init KinectSticks
    public static final MSKinect lstick = new MSKinect(MSKinect.LEFT_ARM);
    public static final MSKinect rstick = new MSKinect(MSKinect.RIGHT_ARM);
    // Jaguars
    public static final Jaguar frontLeft = new Jaguar(RobotMap.Jaguar.FRONT_LEFT);
    public static final Jaguar frontRight = new Jaguar(RobotMap.Jaguar.FRONT_RIGHT);
    public static final Jaguar backLeft = new Jaguar(RobotMap.Jaguar.BACK_LEFT);
    public static final Jaguar backRight = new Jaguar(RobotMap.Jaguar.BACK_RIGHT);
    public static final Jaguar shootTop = new Jaguar(RobotMap.Jaguar.SHOOTER_TOP);
    public static final Jaguar shootBot = new Jaguar(RobotMap.Jaguar.SHOOTER_BOTTOM);
    public static final Jaguar pickup = new Jaguar(RobotMap.Jaguar.PICKUP);
    // Servos
    public static final Servo dropper = new Servo(RobotMap.Servo.DROPPER);
    public static final Servo shootPitch = new Servo(RobotMap.Servo.PITCH);
    public static final Servo shootYaw = new Servo(RobotMap.Servo.YAW);
    // Other
    public static final DigitalOutput led = new DigitalOutput(RobotMap.LED_SPIKE);
    // Vars
    private double power = 0.5;
    private boolean inverse = false;
    public Dashboard dash;
    public void robotInit(){
        // Connect To Dashboard And Update Periodically
        new Thread(){
            public void run() {
                dash = new Dashboard(null);
                while (true) {
                    dash.updatePWM("Output",frontLeft.getChannel(),"Front Left",format(frontLeft.get(),2));
                    dash.updatePWM("Output",frontRight.getChannel(),"Front Right",format(frontRight.get(),2));
                    dash.updatePWM("Output",backLeft.getChannel(),"Back Left",format(backLeft.get(),2));
                    dash.updatePWM("Output",backRight.getChannel(),"Back Right",format(backRight.get(),2));
                    dash.updatePWM("Output",shootTop.getChannel(),"Shooter Top",format(shootTop.get(),2));
                    dash.updatePWM("Output",shootBot.getChannel(),"Shooter Bot",format(shootBot.get(),2));
                    dash.updatePWM("Output",pickup.getChannel(),"Pickup",format(pickup.get(),2));
                    dash.updatePWM("Output",dropper.getChannel(),"Dropper",format(dropper.get(),2));
                    dash.updatePWM("Output",shootYaw.getChannel(),"Shooter Yaw",format(shootYaw.get(),2));
                    dash.updatePWM("Output",shootPitch.getChannel(),"Shooter Pitch",format(shootPitch.get(),2));
                    // 100ms delay
                    Timer.delay(0.1);
                }
            }
            public String format(double num, double dec){
                // Format double num to decimal places dec and return as a string
                return ""+((int)num*(dec=MathUtils.pow(10,dec)))/dec;
            }
        }.start();
    }
    public void autonomous(){
        // If not connected to dashboard or dashboard is set to Kinect Mode and Kinect enabled
        if ((dash==null||dash.requestInfo("Kinect").equals("1"))&&lstick.isEnabled()) {
            // Reset Vars
            reset();
            while (isEnabled()&&isAutonomous()) {
                // Kinect MainLoop
                // Toggle Power With Left Leg Back
                power=lstick.getOption(MSKinect.Button.LEFT_LEG_BACK)?1:0.5;
                // Toggle Inverse With Right Leg Back
                inverse=lstick.getOption(MSKinect.Button.RIGHT_LEG_BACK);
                // Drive Using Arms
                Actions.Auto.drive(lstick.getY(), rstick.getY(), power, inverse);
                // Toggle Pickup With Head Right
                Actions.Auto.pickup(lstick.getOption(MSKinect.Button.HEAD_RIGHT));
                // Turn Dropper With Right Leg Forward
                Actions.Auto.dropper(lstick.isPressed(MSKinect.Button.RIGHT_LEG_FWD));
                // Toggle LED With Head Left
                led.set(lstick.getOption(MSKinect.Button.HEAD_LEFT));
                // Shoot With Left Leg Forward At Highest Hoop
                if (shooter.wasReleased(MSKinect.Button.LEFT_LEG_FWD)) Actions.Auto.shoot(Hoop.HIGHEST);
                // Shoot Without AutoAim With Left Leg Out
                if (shooter.wasReleased(MSKinect.Button.LEFT_LEG_OUT)) Actions.Auto.aimlessShoot(0.5);
                // Delay To Prevent Overload
                Timer.delay(0.05);
            }
        } else {
            // If dashboard is set to Auto Mode or Kinect is disabled
            while (isEnabled()&&isAutonomous()) {
                Actions.Auto.shoot(Hoop.HIGHEST); // Shoot For Highest
            }
        }
    }
    public void operatorControl() {
        // Reset Vars
        reset();
        // Lower Dropper On 'Driver A' Press and Raise On 'Driver A' Release
        driver.addButtonListener(DualAction.Button.A, new Controller.ButtonListener() {
            public void onPress(boolean opt) {
                Actions.Auto.dropper(true);
            }
            public void onRelease(boolean opt) {
                Actions.Auto.dropper(false);
            }
        });
        // Shoot At Hoop hoop on 'Shooter RT' Release
        // with hoop selection using the dpad and def of HIGHEST hoop
        shooter.addButtonListener(DualAction.Button.RIGHT_TRIGGER, new DualAction.ButtonListener() {
            public void onPress(boolean opt) {
            }
            public void onRelease(boolean opt) {
                int hoop = Hoop.HIGHEST; // Def to HIGHEST hoop
                double x=shooter.getRawAxis(DualAction.Axis.DPAD_X); // Get DX
                double y=shooter.getRawAxis(DualAction.Axis.DPAD_Y); // Get DY
                if (x<-0.5) hoop = Hoop.MIDDLE_LEFT; // Set To MIDDLE LEFT
                else if (x>0.5) hoop = Hoop.MIDDLE_RIGHT; // Set To MIDDLE RIGHT
                else if (y<-0.5) hoop = Hoop.TOP; // Set To TOP
                else if (y>0.5) hoop = Hoop.BOTTOM; // Set To BOTTOM
                Actions.Manual.shoot(hoop); // Shoot
            }
        });
        while (isEnabled()&isOperatorControl()){
            // Teleop MainLoop
            // Turbo is toggled with 'Driver LT'
            power=driver.getOption(DualAction.Button.LEFT_TRIGGER)?1:0.5;
            // Inverse is toggled with 'Driver RT'
            inverse=driver.getOption(DualAction.Button.RIGHT_TRIGGER);
            // Drive
            Actions.Manual.drive(power,inverse);
            // Pickup is toggled with 'Driver RB'
            Actions.Auto.pickup(driver.getOption(DualAction.Button.RIGHT_BUMPER));
            // LED is toggled with 'Shooter A'
            led.set(shooter.getOption(DualAction.Button.A));
            // Shoot Without AutoAim with 'Shooter LT'
            if (shooter.wasReleased(DualAction.Button.LEFT_TRIGGER)) Actions.Manual.manualShoot(0.5);
            // Delay to prevent overload
            Timer.delay(0.05);
        }
    }
    public void disabled() {
        reset();
    }
    public void reset() {
        driver.clearAll();
        shooter.clearAll();
        lstick.clearAll();
        rstick.clearAll();
        power = 0.5;
        inverse = false;
    }
}